#include <cv.h>
#include <math.h>
#include <iostream>
#include <stdio.h>

#define THRES 0.5
#define WHICH_PARAM 9

#define PARAM_1 34.3
#define PARAM_2 1.1
#define PARAM_3 (-1.1)

//Warning: if a and b are both matrices, "a * b" can change a, use *= instead

//theta, alpha, a, s
cv::Mat_<double> denavitMat = (cv::Mat_<double>(7, 4) <<
	0, 0, 33.5, 0,
	0, -90, 0, 0,
	90, 90, 0, 0,
	90, 180, 81.23, -14.1,
	0, 0, -13.89, 78.78,
	0, 90, 0, 0,
	0, 0, 0, 27.5
	);

cv::Mat_<double> vMat = (cv::Mat_<double>(4,1) << 1,1,1,1);

cv::Mat_<double> getM(double theta_deg, double alpha_deg, double a, double s) {

	//std::cout << theta << " " << alpha << std::endl;
	double theta = theta_deg * M_PI / 180;
	double alpha = alpha_deg * M_PI / 180;
	//std::cout << theta << " " << alpha << std::endl;
	cv::Mat_<double> mat = (cv::Mat_<double>(4,4) << 
		cos(theta), -sin(theta)*cos(alpha),  sin(theta)*sin(alpha), a*cos(theta),
		sin(theta),  cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta),
		         0,      		sin(alpha), 			cos(alpha),            s,
				 0,                      0,                      0,            1);
	return mat;
}

int main() {
	//std::cout << vMat << std::endl;
	//std::cout << denavitMat << std::endl;
	cv::Mat m_i = cv::Mat::eye(4, 4, CV_64FC1);
	cv::Mat mulMat = cv::Mat::eye(4, 4, CV_64FC1);
	for (int i = 0; i < denavitMat.rows; i++) {
		double add = (i == 0)? 0:5;
		std::cout << "i = " << i << ": ";
		cv::Mat newM = getM(denavitMat(i, 0) + add, denavitMat(i, 1), denavitMat(i, 2), denavitMat(i, 3));
		//mulMat = m_i * newM;
		mulMat = m_i;
		mulMat *= newM;
		cv::Mat_<double> v_i = mulMat;
		v_i *= vMat;

		if (i < WHICH_PARAM) {
			m_i = mulMat;
			
			std::cout << v_i << std::endl;
			continue;
		} else if (i > WHICH_PARAM) {
			return 0;
		}

		for (double r1 = 0; r1 < 360; r1+=10) {
			for (double r2 = 0; r2 < 360; r2+=10) {
				for (double j = 0; j < 100; j+=0.5) {
					for (double k = 0; k < 100; k+=0.5) {
						newM = getM(add + r1, r2, j, k);
						//cv::Mat newM = getM(denavitMat(i, 0) + add + r1, denavitMat(i, 1) + r2, denavitMat(i, 2) + j, denavitMat(i, 3) + k);
						
						//mulMat = m_i * newM;
						mulMat = m_i.clone();
						mulMat *= newM;
						//cv::gemm(m, newM, 1, NULL, 0, mulMat);
						//v_i = mulMat * vMat;
						v_i = mulMat;
						v_i *= vMat;

						//std::cout << m_i.at<double>(0,0) << std::endl;
						//std::cout <<  m * getM(15, 180, 82, 13) * vMat << std::endl;
						if (fabs(v_i(0,0) - PARAM_1) < THRES && fabs(v_i(1,0) - PARAM_2) < THRES && fabs(v_i(2,0) - PARAM_3) < THRES) {
							std::cout << "vals were " << r1 << " " << r2 << " " << j << " " << k << std::endl;	
							std::cout << v_i << std::endl;
						}
					}	
				}
			}
		}
	}
	return 0;
}